// ==============================================
// =      U.A.V.P Brushless UFO Controller      =
// =           Professional Version             =
// = Copyright (c) 2007 Ing. Wolfgang Mahringer =
// ==============================================
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with this program; if not, write to the Free Software Foundation, Inc.,
//  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
// ==============================================
// =  please visit http://www.uavp.org          =
// =               http://www.mahringer.co.at   =
// ==============================================


// CC5X Compiler parameters:
// -CC -fINHX8M -a -L -Q -V -FM -DMATHBANK_VARS=bank0 -DMATHBANK_PROG=2


#ifdef ICD2_DEBUG
#pragma	config = 0x377A	// BODEN, HVP, no WDT, MCLRE disabled, PWRTE disabled
#else
#pragma	config = 0x3F72	// BODEN, HVP, no WDT, MCLRE disabled, PWRTE enabled
#endif

#include "c-ufo.h"
#include "bits.h"

#pragma codepage = 0

// Math Library
#include "mymath16.h"


// EEPROM-Data can currently not be used with relocatable assembly
// CC5X limitation!
// #define EEPROM_START	0x2100
// #pragma cdata[EEPROM_START] = 0,0,0,0,0,0,0,0,0,0,20,8,16

// The globals

uns8	IGas;			// actual input channel, can only be positive!
int 	IRoll,IPitch,IYaw;	// actual input channels, 0 = neutral
uns8	IK5;		// actual channel 5 input
uns8	IK6;		// actual channel 6 input
uns8	IK7;		// actual channel 7 input

// PID Regler Variablen
int		Roll_Rate_8,Pitch_Rate_8;
int 	Yaw_Rate_8;	// Fehlersignal aus dem aktuellen Durchlauf
int		Roll_Rate_8_prev,Pitch_Rate_8_prev;

int		Yaw_Rate_8_prev;	// Fehlersignal aus dem vorigen Durchlauf (war Roll_Rate_8/Pitch_Rate_8/Yaw_Rate_8)
long	Yaw_Angle;	// Integrales Fehlersignal fuer Yaw, 0 = neutral
long	Roll_Angle, Pitch_Angle;	// Integrales Fehlersignal fuer Roll und Pitch
int16	RollSamples, PitchSamples,YawSamples;
long	UpDown_Velocity;
int		LRIntKorr, FBIntKorr;
uns8	NeutralLR, NeutralFB, NeutralUD;


int		NegFact; // general purpose

uns8	BlinkCount;

long	niltemp1;
long	niltemp;
int		BatteryVolts; // added by Greg Egan
int		Rw,Nw;

uns16	BasePressure, BaseTemp, TempCorr;
int		VBaroComp;
long 	BaroCompSum;


// Die Reihenfolge dieser Variablen MUSS gewahrt bleiben!!!!
int	RollPropFactor;
int	RollIntFactor;
int	RollDiffFactor;
int RollLimit;
int RollIntLimit;

int	PitchPropFactor;
int	PitchIntFactor;
int	PitchDiffFactor;
int PitchLimit;
int PitchIntLimit;

int	YawPropFactor;
int	YawIntFactor;
int	YawDiffFactor;
int	YawLimit;
int YawIntLimit;

int	ConfigParam;
int overwritebuff;
int	Interval_time;
int LowVoltThres;

int	LinLRIntFactor;
int	LinFBIntFactor;
int	LinUDIntFactor;
int MiddleUD;
int MotorLowRun;
int	MiddleLR;
int MiddleFB;
int	CamPitchFactor;
int CompassFactor;

int BaroThrottleDiff;

// Ende Reihenfolgezwang

uns16	RollBias, PitchBias, YawBias;

uns8	LedShadow;	// shadow register
uns16	AbsDirection;	// wanted heading (240 = 360 deg)
int		CurDeviation;	// deviation from correct heading


uns8	MVorne,MLinks,MRechts,MHinten;	// output channels
uns8	MCamRoll,MCamPitch;
long	Ml, Mr, Mv, Mh;
long	Rl,Nl,Tl;	// PID output values
long	Rp,Np,Tp;
long	Vud;

uns8	Flags;
uns8	Flags2;

uns8	BiasZeroCount;
int		RollNeutral, PitchNeutral, YawNeutral;

uns8	ThrNeutral;
uns8	ThrDownCount;

void SetDefaultRX_PWM(void) 
{
	// some resonable input channels values  
	if ( !FutabaMode )
	{	
		RXCh_Val[0] =_Minimum;	
		RXCh_Val[2] =_Neutral_In;
	}
	else
	{
		RXCh_Val[2] =_Minimum;	
		RXCh_Val[0] =_Neutral_In;
	}
	
	RXCh_Val[1] =_Neutral_In;	
	RXCh_Val[3] =_Neutral_In;	
	RXCh_Val[4] =_Minimum;		
	RXCh_Val[5]= _Neutral_In;	
	RXCh_Val[6]= _Neutral_In;			
	RXCh_Val[7]= _Neutral_In;	
	RXCh_Val[8]= _Neutral_In;
}
void SetMotorsOff( void )
{
	// all motors off -- Cam netral 
	PWM_Val[0] = 0;  
	PWM_Val[1] = 0; 	
	PWM_Val[2] = 0;
	PWM_Val[3] = 0;
	
	PWM_Val[4] = _Neutral_Out;
	PWM_Val[5] = _Neutral_Out;
}
void GetInputCH( void )
{	// do channel  assignenment in here 
	// possibly do _NoSignal processing in here . i.e. assigne fixed values -- try to bring UFO down safely 
	if (_NoSignal )
		SetDefaultRX_PWM();
	
//	if ( !FutabaMode )
	{
		IGas  = RXCh_Val[0];
		IRoll = RXCh_Val[1]  - _Neutral_In;
		IPitch = RXCh_Val[2] - _Neutral_In;
		IYaw  = RXCh_Val[3]  - _Neutral_In;
	}
/*	else
	{
		IRoll = RXCh_Val[0]  - _Neutral_In;
		IPitch = RXCh_Val[1] - _Neutral_In;
		IGas  = RXCh_Val[2];
		IYaw  = RXCh_Val[3]  - _Neutral_In;
	}
*/	
	IK5   = RXCh_Val[4];		
	IK6   = RXCh_Val[5];		
	IK7   = RXCh_Val[6];
	
	if (DoubleRate )
	{
		IRoll = IRoll/2;
		IPitch = IPitch/2;
	}		
		
	
	
}

void main(void)
{
	uns8	DropoutCount;
	int		nii@NegFact;
	uns8	LowGasCount;
	uns8	LedCount;
	

	RBPU_ = 1;				// enable weak pullups  -- 
	
	// general ports setup
	TRISA = 0b.00111111;	// all inputs
	ADCON1 = 0b.0.000.0010;	// uses 5V as Vref

	PORTB = 0b.1100.0000;	// all outputs to low, except RB6 & 7 (I2C)!
	TRISB = 0b.0100.0000;	// all servo and LED outputs

	PORTC = 0b.0110.0000;	// all outputs to low, except TxD and CS
	TRISC = 0b.10000100;	// RC7, RC2 are inputs

	CKE = 1;				// default I2C - enable SMBus thresholds for 3.3V LIS

//GS: why is ledshadow not inside the macros ????
	LedShadow = 0;
    ALL_LEDS_OFF;

// setup serial port for 8N1
	TXSTA = 0b.0010.0100;	// async mode, BRGH = 1
	RCSTA = 0b.1001.0000;	// receive mode
	SPBRG = _B38400;
	W = RCREG;			// be sure to empty FIFO

// setup flags register
	Flags = 0;
	Flags2 = 0;

	LedRed_ON;		// red LED on
	
	SetMotorsOff();
	SetDefaultRX_PWM();

	PWM_Pause =10; 		// Pause in ms in between pulses, 6ms for the 6 PWM pulse + Pause == PWM repeat time. 

	_NoSignal = 1;		// assume no signal present
	
	// Timer0 setup 
	OPTION_REG = 0b.10000.011;	// TMR0 w/ int clock, 1:16 presc (see _PreScale0),  Ticks at 4us, interrupts every 1024us ,  ALSO : Weak PULLUPS
	T0IF = 0;
	T0IE = 1;

	// Timer2 setup
	T2CON = 0b.00000111; // enable the timer at 4us per tick, i.e Fosc/4/16
	PR2 = 0xFF; 
	TMR2IF = 0;
	TMR2IE = 0;

	// Timer1 setup
	TMR1L = 0;
	TMR1H = 0;
	T1CON = 0b.00.11.0001;	// enable TMR1, 1:8 prescaler, run with 2us clock  == maxrun of 0xffff x 2us = 130ms
	TMR1IF = 0;
	TMR1IE = 1;

	// Capture 
	CCP1CON = 0b.00.00.0100;	// capture mode every falling edge
	CCP1IE = 1;

	// Enable interrrupts 
	PEIE   = 1;
	GIE    = 1; 

#ifdef USE_ACCSENS
	IsLISLactive();
	if( _UseLISL )
	{
		LedYellow_ON;	
		GetEvenValues();	// into Rp, Np, Tp
	}
	else
	{
		NeutralLR = 0;
		NeutralFB = 0;
		NeutralUD = 0;
	}
#endif

    InitDirection();	// init compass sensor
	InitAltimeter();
	ShowSetup(1);
	
// gs: you get here when 
// a.)  ufo has been flying and remote signal has been lost for more than Dropout count	
	while(1)
	{
		IGas =IK5 = _Minimum;	// assume parameter set #1
		
		Beeper_OFF;
		ALL_LEDS_OFF;

		InitArrays();
		SetMotorsOff(); 			// stop motors 
		ThrNeutral = 0xFF;

		DropoutCount = MODELLOSTTIMER;
		nitemp=0;
		do
		{
			Delay_ms(50);
			ProcessComCommand(); 
			LedGreen_TOG;    // quick feedback to indication SW is waiting for the right strting conditions
			Delay_ms(50);
			ProcessComCommand(); 
			
			GetInputCH();   // copy the input channels into the IK vars -- needed for IGas only in here 		
			if ( Switch )  // Switch is on 
			{
				if (_NoSignal )
				{
					if( --DropoutCount == 0 )
					{
						Beeper_TOG;	// toggle beeper "model lost"
						DropoutCount = MODELLOSTTIMERINT;
					}
					nitemp = 0;
				}
				else
				{ 
					Beeper_OFF;
					if( IGas >= _ThresStop) // No starting if Throttle was not at low pos
					{
						LedRed_TOG;	// toggle red Led  indicating  Throttle not off
						nitemp=0;
					}	
					else
						nitemp++;
				}
				
			}else // Switch is off
			{
				DropoutCount = MODELLOSTTIMER;
				Beeper_OFF;
				nitemp=0;
			}
		}
		while( _NoSignal || !Switch || nitemp < 4);	// no signal or switch is off or throttle is not closed 
		Beeper_OFF;

		ReadEEdata();

		PWM_Pause = Interval_time;   

if( IK5 > _Neutral_In )
	_UseParam1=0;
else
	_UseParam1=1;
	
// ###############
// ## MAIN LOOP ##
// ###############
		DropoutCount = 0;
		BlinkCount = 0;
		BiasZeroCount = BiasZeroAvgCount;	// do 16 cycles to find integral zero point
		ThrDownCount = THR_DOWNCOUNT;
		PID_Delay = 0;

			
		while(Switch == 1)	// as long as power switch is ON
		{
			GetInputCH();   // copy the input channels into the IK vars 	

			ProcessComCommand(); 
		

// GS:  Only read params from the EEPROM when switch on CH5 has changed - this saves about 192us in the loop 
			if( (IK5 > _Neutral_In &&  _UseParam1   )|| (IK5 < _Neutral_In  && !_UseParam1  )   )
			{ 	 
				ReadEEdata();	// re-sets Interval_time
				PWM_Pause = Interval_time;
				
				if (IK5 > _Neutral_In )
					_UseParam1=0; 
				else
					_UseParam1=1;
			}
			
// Setup Blink counter
			if( BlinkCount == 0 )
				BlinkCount = BLINK_LIMIT;
			BlinkCount--;

			PID_Sleep();		// Sleeps until the PID delay is expired..  ISR managed counter based on Tmr0 -- 
								// Jitter free, not affected by execution  time of code in PID loop
			
			GetGyroValues();	// first thing after sleep so that there is not jitter  in dt.
			CheckLISL(); 		// Read the accel  and  compute the correction factors 
			CalcGyroValues();	// makes 8 bit and 10  bit Gyro ADC values , removes Bias, integrates samples into Roll_Angle, Pitch_Angle.Yaw_Angle applies correction factors to angles 

// 			Roll_Rate_8 , Pitch_Rate_8 now have Delta Theta of Roll and Pitch in 8 bit representation
// 			Roll_Rate_8_prev , Pitch_Rate_8_prev now have the PREVIOUS Delta Theta 
// 			RollSamples, PitchSamples now have Delta Theta in 10 bit representation  -- used to integrate to Roll_Angle. Pitch_Angle
//                            Roll_Angle, Pitch_Angle  now have Current  roll an pitch angles 			

 
			
			if( _UseCompass && ((BlinkCount & 0x03) == 0) )	// enter every 4th scan
			{
				GetDirection();	// read compass sensor
			}

			if( _UseBaro )
				ComputeBaroComp();

			// check for signal dropout while in flight
			if( _NoSignal && _Flying ) 
			{
				DropoutCount++ ;
				if ( DropoutCount > 60 )
					break; // GS: this break goes back to the outermost loop , Stops everything 
			
// GS: FOR NOW : shut the power  off upon loosing the TX signal -- Ultimately it should bring down the UFO in a safe manner
// GS:  note possible delay in detecting NoSiganal up tp 128 ms  with current timer1 setup and f_osc at 16mhz

				// for a while we hope that the signal comes back ... ?????? 
				IGas = _Minimum;		
				IRoll = RollNeutral;	// these are the values that where recorded upon takeoff and represent trimmed stick values
				IPitch = PitchNeutral;
				IYaw = YawNeutral;
				goto DoPID;
			}
// allow motors to run on low throttle 
// even if stick is at minimum for a short time
			if( _Flying && (IGas <= _ThresStop) )
			{
				if( --LowGasCount > 0 )
				{
					goto DoPID;
				}
			}

			if( (  _Flying && (IGas <= _ThresStop)) ||  // ufo is landed  -- hopefully
			     (!_Flying && (IGas <= _ThresStart)) )  // ufo has not taken off yet
			{	// stop all motors
				SetMotorsOff();
				ThrDownCount = THR_DOWNCOUNT;
				InitArrays();	// resets _Flying flag!  All  angles are set to 0 in here   !!  ASSUMING LEVEL !! ground of UAVP 
				
				BiasZeroCount = BiasZeroAvgCount;	// do 16 cycles to find integral zero point
				RollBias = PitchBias = YawBias = 0;	// gyro neutral points


				ALL_LEDS_OFF;
				AUX_LEDS_OFF;
				LedGreen_ON;	 // This steady Green indicates that we have transitioned into ready to fly mode 
			}
			else
			{	// UFO is  about to be flying!
				if( !_Flying )	// about to start
				{	// set current stick values as midpoints
					RollNeutral = IRoll;
					PitchNeutral = IPitch;
					YawNeutral  = IYaw;
					AbsDirection = COMPASS_INVAL;
					LedCount = 1;
					UpDown_Velocity = 0;
				}
				// ufo is flying
				_Flying = 1;
				
				if( DropoutCount )
					Beeper_OFF;	// turn off signal lost beeper
				
				DropoutCount = 0;
				LowGasCount = 100;
				--LedCount;

DoPID:
// do the calculations

// this block checks if throttle stick has moved for Barometer altitude lock , supposed to be 3 sec until lock takes effect 
// after last stick movement 
				if( ThrDownCount > 0 )
				{
					if( LedCount.0 == 0 )
						ThrDownCount--;
					if( ThrDownCount == 0 )
						ThrNeutral = IGas;	// remember current Throttle level
				}
				else
				{
					if( ThrNeutral < THR_MIDDLE )
						NegFact = 0;
					else
						NegFact = ThrNeutral - THR_MIDDLE;
					if( IGas < THR_HOVER )
						ThrDownCount = THR_DOWNCOUNT;	// left dead area
					if( IGas < NegFact )
						ThrDownCount = THR_DOWNCOUNT;	// left dead area
					if( IGas > ThrNeutral + THR_MIDDLE )
						ThrDownCount = THR_DOWNCOUNT;	// left dead area
				}

				PID();
				MixAndLimit();
				// remember old gyro values for next itteration , used with Derivative factor
				Roll_Rate_8_prev = Roll_Rate_8;      
				Pitch_Rate_8_prev = Pitch_Rate_8;
				Yaw_Rate_8_prev = Yaw_Rate_8;
			}
	
			MixAndLimitCam();	// Always mix the Camera Servos, even when not flying.
			OutSignals();		// Output the results to the speed controllers

// indicate problems 
			GetVbattValue();		// Get Battery status 
			LedYellow_OFF;

			if( _LowBatt || _NoSignal) {Beeper_ON;}
			else{Beeper_OFF;}
				
			if (_NoSignal ){LedBlue_ON;}       // flash the blue LED while we experiencing RX dropout  
			else { LedBlue_OFF;}
			
			if( _LowBatt ){	LedRed_ON;}
			else {LedRed_OFF;}

		}	// End  while (Switch)
	}  // End  while ( 1 )
}

